#include <iostream>
#include "hessian.h"

using namespace std;

std::vector<std::vector<double> > hess(std::valarray<double> w, double (*func)(std::valarray<double>,file_names& Fnames,input& data),
                                input& data, file_names& Fnames) {
    double eta_min = 1e-4; //1e-4
    unsigned n = w.size();
    double epsilon = data.epsilon;

    std::valarray<double> d(n), wm(n), wp(n), fm(n), fp(n);
    std::vector<std::vector<double> > H(n,std::vector<double> (n));
    double tmp1, tmp2, f2p, h, f;
    if (epsilon < eta_min) epsilon = eta_min;
    h = epsilon;
    f = func(w,Fnames,data);

    for (unsigned i=0; i<n; i++) {
        if (w[i]!=0) d[i] = h*w[i];
        else d[i] = h;
        wp[i] = w[i]+d[i];
        wm[i] = w[i]-d[i];
    }

    for (unsigned i=0; i<n; i++) {
        tmp1 = w[i];
        w[i] = wm[i];
        fm[i] = func(w,Fnames,data);
        w[i] = wp[i];
        fp[i] = func(w,Fnames,data);
        w[i] = tmp1;
    }

    for (unsigned i=0; i<n; i++) {
        H[i][i] = (fp[i] + fm[i] - 2*f)/(d[i]*d[i]);
    }

    for (unsigned i=0; i<n-1; i++) {
        tmp1 = w[i];
        w[i] = wp[i];
        for (unsigned j=i+1; j<n; j++) {
            tmp2 = w[j];
            w[j] = wp[j];
            f2p = func(w,Fnames,data);
            H[i][j] = (f2p - fp[i] - fp[j] + f)/(d[i]*d[j]);
            H[j][i] = H[i][j];
            w[j] = tmp2;
        }
        w[i] = tmp1;
    }

/*
    for (unsigned i=0; i<n; i++) {
        for (unsigned j=0; j<n; j++) {
            std::cout << "H" << "[" << i << "]" << "[" << j << "] = " << H[i][j] << std::endl;
        }
    }

    std::cout << std::endl;
*/
    return H;
}


std::valarray<double> product_mv(std::vector<std::vector<double> > matr, std::valarray<double> vect) {
    unsigned n = vect.size();

    std::valarray<double> mv(n);
    for (unsigned i=0; i<n; i++) {
        for (unsigned j=0; j<n; j++) {
            mv[i] += matr[i][j]*vect[j];
        }
    }

/*
    for (unsigned i=0; i<mv.size(); i++) {
        std::cout << "mv[" << i <<"]= " << mv[i] << std::endl;
    }

    std::cout << std::endl;
*/

    return mv;
}


double product_vec(std::valarray<double> vec1, std::valarray<double> vec2) {
    double res = 0;
    if (vec1.size()!=vec2.size()) std::cout << "not eq!" << endl;
    for (unsigned i=0; i<vec1.size(); i++) res += vec1[i]*vec2[i];
    return res;
}
